#include "hnurm_uart/uart_node.hpp"

#include <Eigen/Core>
#include <Eigen/Dense>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_eigen/tf2_eigen.hpp>

#include <angles/angles.h>
#include <filesystem>

using namespace std::chrono_literals;

namespace hnurm
{
void UartNode::run()
{
    // check if /dev/serial/by-id/ is created
    while(!std::filesystem::exists("/dev/serial/by-id/"))
    {
        RCLCPP_WARN(logger, "Waiting for /dev/serial/by-id/ to be created");
        std::this_thread::sleep_for(1s);
    }

    recv_topic_ = this->declare_parameter("recv_topic", "vision_recv_data");
    send_topic_ = this->declare_parameter("send_topic", "vision_send_data");
    control_id_ = static_cast<float>(this->declare_parameter("control_id", 1.0f));

    serial_codec_    = new SerialCodec(shared_from_this());
    callback_group1_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
    callback_group2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    pub_             = create_publisher<hnurm_interfaces::msg::VisionRecvData>(recv_topic_, rclcpp::SensorDataQoS());

    auto sub_option           = rclcpp::SubscriptionOptions();
    sub_option.callback_group = callback_group2_;
    sub_                      = create_subscription<hnurm_interfaces::msg::VisionSendData>(
        send_topic_,
        rclcpp::ServicesQoS(),
        std::bind(&UartNode::sub_callback, shared_from_this(), std::placeholders::_1),
        sub_option
    );
    sub_twist_ = create_subscription<geometry_msgs::msg::Twist>(
        "cmd_vel",
        rclcpp::ServicesQoS(),
        std::bind(&UartNode::sub_twist_callback, shared_from_this(), std::placeholders::_1),
        sub_option
    );

    // tf
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(shared_from_this());

    uart_thread_ = std::thread([this]() {
        while(rclcpp::ok() && !stop_flag_)
        {
            timer_callback();
        }
        RCLCPP_WARN(logger, "uart thread exit");
    });
}

void UartNode::sub_callback(hnurm_interfaces::msg::VisionSendData::SharedPtr msg)
{
    msg->vel_x   = 2000.0;
    msg->vel_y   = 2000.0;
    msg->vel_yaw = 2000.0;
    if(serial_codec_->send_data(*msg))
        RCLCPP_INFO(logger, "send data");
}

void UartNode::sub_twist_callback(geometry_msgs::msg::Twist::SharedPtr msg)
{
    hnurm_interfaces::msg::VisionSendData send_data;
    send_data.vel_x   = static_cast<float>(msg->linear.x);
    send_data.vel_y   = static_cast<float>(msg->linear.y);
    send_data.vel_yaw = static_cast<float>(msg->angular.z);
    if(serial_codec_->send_data(send_data))
        RCLCPP_INFO(logger, "send data");
}

void UartNode::timer_callback()
{
    hnurm_interfaces::msg::VisionRecvData recv_data;
    if(serial_codec_->try_get_recv_data_for(recv_data))
    {

        // 单云台机器人不需要验证
        //        if(control_id_ != recv_data.control_id)
        //        {
        //            RCLCPP_WARN(logger, "control id not match, ignoring this msg %f[expected]!=%f[recv]", control_id_,
        //            recv_data.control_id); return;
        //        }

        if(recv_data.self_color.data == hnurm_interfaces::msg::SelfColor::COLOR_NONE)
        {
            RCLCPP_WARN(logger, "self color not set, ignoring this msg");
            return;
        }

        // pub original data
        recv_data.header.stamp    = now();
        recv_data.header.frame_id = "serial";
        pub_->publish(recv_data);
        RCLCPP_INFO(logger, "recv data: %f, %f, %f", recv_data.pitch, recv_data.yaw, recv_data.roll);
        // pub transform
//        geometry_msgs::msg::TransformStamped transform;
//        transform.header.stamp    = now();
//        transform.header.frame_id = "gimbal_link";
//        transform.child_frame_id  = "imu_link";
//
//        tf2::Quaternion    q;
//        Eigen::Quaternionf q_eigen;
//        q.setEuler(
//            angles::from_degrees(recv_data.yaw),    //
//            angles::from_degrees(recv_data.pitch),  //
//            angles::from_degrees(recv_data.roll)
//        );
//        transform.transform.rotation.x    = q.x();
//        transform.transform.rotation.y    = q.y();
//        transform.transform.rotation.z    = q.z();
//        transform.transform.rotation.w    = q.w();
//        transform.transform.translation.x = 0.0;
//        transform.transform.translation.y = 0.0;
//        transform.transform.translation.z = 0.0;
//        // pub
//        tf_broadcaster_->sendTransform(transform);
    }
    else
    {
        std::this_thread::sleep_for(10ms);
    }
    else
    {
        if(error_cnt_++ > 100)
        {
            std::thread([this]() { re_launch(); }).detach();
        }
        std::this_thread::sleep_for(10ms);
    }
}

void UartNode::re_launch()
{
    stop_flag_ = true;
    uart_thread_.join();
    stop_flag_ = false;

    // check if /dev/serial/by-id/ is created
    while(!std::filesystem::exists("/dev/serial/by-id/"))
    {
        RCLCPP_WARN(logger, "Waiting for /dev/serial/by-id/ to be created");
        std::this_thread::sleep_for(1s);
    }

    error_cnt_ = 0;
    serial_codec_->init_port();
    uart_thread_ = std::thread([this]() {
        while(rclcpp::ok() && !stop_flag_)
        {
            timer_callback();
        }
        RCLCPP_WARN(logger, "uart thread exit");
    });
}

}  // namespace hnurm
